/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.ncsu.gps;

import com.sun.spot.core.peripheral.IDriver;
import com.sun.spot.core.peripheral.Spot;
import com.sun.spot.core.peripheral.TimeoutException;
import com.sun.spot.core.resources.transducers.IOutputPin;
import com.sun.spot.core.resources.transducers.LEDColor;
import com.sun.spot.core.util.Utils;
import com.sun.spot.edemo.EDemoBoard;
import java.io.IOException;
import edu.ncsu.util.LEDManager;
import edu.ncsu.util.MyStringTokenizer;
import edu.ncsu.gps.nmea.NMEAString;

/**
 *
 * @author me
 */
public class GPSReader implements IDriver {
    private static final float HDOP_CUTOFF = (float)7.0;
    private GPSLockRecorder rec = new GPSLockRecorder();
    
    private double lastLat = Double.MAX_VALUE;
    private double lastLon = Double.MAX_VALUE;
    private double lastAlt = Double.MAX_VALUE;
    private double hdop = 0;

    private static EDemoBoard demo = EDemoBoard.getInstance();
    private static boolean demoSetup = false;

    private static IOutputPin gpsPower = EDemoBoard.getInstance().getOutputPins()[EDemoBoard.H0];
    private boolean GPSActive = false;

    private boolean preventSleep = false;

    private int tasks = 0;

    private GPSReader() {
        if ("true".equals(Spot.getInstance().getPersistentProperty("Ranging"))) {
            setUp();
            Spot.getInstance().getDriverRegistry().add(this);
        }
    }

    private static GPSReader reader = new GPSReader();

    public static GPSReader getInstance() {
        return reader;
    }

    public boolean detectGPS() {
        if (GPSActive) return true;
        synchronized (this) {
            try {
                initUART();
                demo.readUART(3000);
                return true;
             } catch (TimeoutException tex) {
                 return false;
             }catch (IOException ex) {
                ex.printStackTrace();
                return detectGPS();
             }
        }
    }

    private void writeOutString(String s) throws IOException {
        if (s == null) {
            return;
        }
        System.out.println(s);
        for (int i=0; i<s.length(); i++) {
            System.out.print(s.charAt(i));
            demo.writeUART((byte)s.charAt(i));
        }
        System.out.println();
    }

    private void initUART() {
        gpsPower.setHigh();
        synchronized (this) {
            if (!demoSetup) {
                demo.initUART(EDemoBoard.SERIAL_SPEED_4800,EDemoBoard.SERIAL_DATABITS_8,EDemoBoard.SERIAL_PARITY_NONE,EDemoBoard.SERIAL_STOPBITS_2);
            }
        }
    }

    private void initGPS() {
        initUART();
        try {
            clearUART();
            writeOutString(NMEAString.NMEAInitializationString(lastLat, lastLon, lastAlt));
        } catch (IOException ex) {
            ex.printStackTrace();
        }
    }

    private boolean readGGA() {
        NMEAString gpsData = new NMEAString();
        while (!gpsData.isValid()) {
            gpsData = readMessage();
        }
        System.out.println(gpsData.getNMEAString());
        return readGGA(gpsData.getNMEAString());
    }

    private boolean readGGA(String gpsData) {
        System.out.println(gpsData);
        MyStringTokenizer tokenizer = new MyStringTokenizer(gpsData,",");
        String label = tokenizer.nextToken();
        System.out.println(label);
        if (label.equals("$GPGGA")) {
            try {
                String time = tokenizer.nextToken();
                System.out.println("UTC Time = " + time);
                String latitude = tokenizer.nextToken();
                if (latitude == null) {
                    failedGGA();
                    return false;
                }
                String nsew = tokenizer.nextToken();
                System.out.println("Latitude = " + latitude + " " + nsew);
                if ("".equals(latitude)) {
                    failedGGA();
                    return false;
                }
                lastLat = Double.parseDouble(latitude);
                if (nsew.equals("S")) {
                    lastLat *= -1;
                }
                String longitude = tokenizer.nextToken();
                if (longitude == null) {
                    failedGGA();
                    return false;
                }
                nsew = tokenizer.nextToken();
                System.out.println("Longitude = " + longitude + " " + nsew);
                if ("".equals(longitude)) {
                    failedGGA();
                    return false;
                }
                lastLon = Double.parseDouble(longitude);
                if (nsew.equals("W")) {
                    lastLon *= -1;
                }
                if (Integer.parseInt(tokenizer.nextToken()) == 0) {
                    failedGGA();
                    return false;
                }
                tokenizer.nextToken();
                String hdopString = tokenizer.nextToken();
                if ("".equals(hdopString)) {
                    failedGGA();
                    return false;
                }
                hdop = Double.parseDouble(hdopString);
                System.out.println("HDOP = " + hdop);
                if (hdop > HDOP_CUTOFF) {
                    failedGGA();
                    return false;
                }
                String altitude = tokenizer.nextToken();
                if ("".equals(altitude)) {
                    failedGGA();
                    return false;
                }
                lastAlt = Double.parseDouble(altitude);
                return true;
            } catch (Exception e) {
                failedGGA();
                return false;
            }
        } // if
        return false;
    }

    private void failedGGA() {
        LEDManager.getInstance().setColor(4, LEDColor.RED);
        Utils.sleep(1000); //wait for a while to establish a lock
        clearUART();
    }

    private void clearUART() {
        try {
            for (int i=0; i<demo.availableUART()-10; i++) demo.readUART();
        } catch (IOException ex) {
            //ex.printStackTrace();
        }
    }

    private double convert(double value) {
        System.out.println("Converting " + value);
        //do Conversion
        System.out.println("Converted value: " + value);
        return value;
    }

    private NMEAString readMessage() {
        StringBuffer sBuff = new StringBuffer();
        sBuff.append('$');
        byte val = 0;
        System.out.println("receiving...");
        while(true) {
            try {
                val = demo.readUART();
                switch (val) {
                    case 10:
                        return new NMEAString(sBuff.toString());
                    case (byte)('$'):
                        return readMessage();
                    default:
                        sBuff.append((char)val);
                }
            } catch (Exception e) {
                Utils.sleep(10);
            }
        }
    }

    /**
     * Attempts to establish a GPS lock and return GPS reading as double array
     * @param stop - Time in milliseconds to continue to attempt a GPS lock
     * @return 3- double array with latitude and longitude in degrees, and hdop.  Returns null if no lock established in time
     */
    public double[] takeReading(long stop) {
        stop += System.currentTimeMillis();
        if (detectGPS()) {
            LEDManager.getInstance().setColor(4, LEDColor.GREEN);
            System.out.println("GPS Found");
            System.out.println(tasks + " tasks in GPS queue");
            if (!GPSActive) {
                GPSActive = true;
                initGPS();
            }
        } else {
            System.out.println("No GPS Found");
            LEDManager.getInstance().setColor(4, LEDColor.RED);
            return null;
        }
        System.out.println("Running GPSTask for the next " + (stop-System.currentTimeMillis()) + "ms");
        LEDManager.getInstance().setColor(4, LEDColor.YELLOW);
        if (takeReadGGA(stop)) {
            LEDManager.getInstance().setColor(4, LEDColor.GREEN);
            System.out.println("GPS Lock established!");
            lastLat = convert(lastLat);
            lastLon = convert(lastLon);
            endTask();
            double[] theAnswer = {lastLat, lastLon, hdop};
            return theAnswer;
        }
        LEDManager.getInstance().setColor(4, LEDColor.RED);
        endTask();
        return null;
    }

    private boolean takeReadGGA(long stop) {
        LEDManager.getInstance().turnOnLED(4);
        while (!readGGA()) {
            LEDManager.getInstance().turnOnLED(4);
            if (System.currentTimeMillis() > stop) {
                endTask();
                return false;
            }
        }
        return true;
    }

    /**
     * Allows SPOT to sleep
     */
    private void endTask() {
        synchronized (this) {
            GPSActive = false;
        }
    }

    public String getDriverName() {
        return "GPS Reader";
    }

    public boolean tearDown() {
        if (preventSleep) {
            return false;
        }
        if (!GPSActive) {
            gpsPower.setLow();
            return true;
        }
        return false;
    }

    /**
     * reads latitude, longitude and altitude from persistent properties, if they have previously been recorded
     */
    public void setUp() {
        double[] setup = rec.readCoords();
        if (setup != null) {
            lastLat = setup[0];
            lastLon = setup[1];
            lastAlt = setup[2];
        }
    }

    /**
     * records latitude, longitude and altitide to persistent properties
     */
    public void shutDown() {
        rec.recordCoords(lastLat, lastLon, lastAlt);
    }

    /**
     * When set true, GPS stays active even if no demand for readings
     * @param prevent
     */
    public void preventSleep(boolean prevent) {
        preventSleep = prevent;
    }
}
